/**
 * @file ebex_genetic_mutate.c
 *
 * @date Oct 21, 2011
 * @author seth
 *
 * @brief This file is part of EBEX State Estimator, created for the EBEX project
 *
 * This software is copyright (C) 2011 Columbia University
 *
 * EBEX State Estimator is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * EBEX State Estimator is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with EBEX State Estimator; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#include <ebex_genetic.h>
#include <random_util.h>
#include <ebex_quaternion.h>
#include <ebex_genetic_chromosome.h>
#include <ebex_genetic_mutate.h>

void ebex_mutate_double_singlepoint_drift(population_t *m_pop __attribute__((unused)), organism_t *m_organism, int m_chromosome)
{
	int allele;

	if (!m_organism)
		die("Null pointer to entity structure passed");

	/* Select mutation locus. */
	allele = (int) ran_uint_uniform(m_organism->chromosome[m_chromosome].length);

	/*
	 * Mutate by tweaking a single allele.
	 */
	m_organism->chromosome[m_chromosome].dbl_allele[allele] += ran_sample(&m_organism->chromosome[m_chromosome].rnd);

	return;
}

void ebex_mutate_double_singlepoint_randomize(population_t *m_pop __attribute__((unused)), organism_t *m_organism, int m_chromosome)
{
	int allele;

	if (!m_organism)
		die("Null pointer to entity structure passed");

	/* Select mutation locus. */
	allele = ran_uint_uniform(m_organism->chromosome[m_chromosome].length);

	m_organism->chromosome[m_chromosome].dbl_allele[allele] = ran_sample(&m_organism->chromosome[m_chromosome].rnd);
	return;
}

void ebex_mutate_double_multipoint(population_t *m_pop __attribute__((unused)), organism_t *m_organism, int m_chromosome)
{
	size_t allele;

	if (!m_organism)
		die("Null pointer to entity structure passed");
	/*
	 * Mutate by tweaking alleles.
	 */

	for (allele = 0; allele < m_organism->chromosome[m_chromosome].length; allele++)
	{
		if (ran_bool_prob(m_organism->chromosome[m_chromosome].allele_mutation_prob))
		{
			m_organism->chromosome[m_chromosome].dbl_allele[allele] += ran_sample(&m_organism->chromosome[m_chromosome].rnd);

		}
	}

	return;
}


void ebex_mutate_double_allpoint(population_t *m_pop __attribute__((unused)), organism_t *m_organism, int m_chromosome)
{
	size_t allele;

	if (!m_organism)
		die("Null pointer to entity structure passed");

	/*
	 * Mutate by adjusting all alleles.
	 */
	for (allele = 0; allele < m_organism->chromosome[m_chromosome].length; allele++)
	{
		m_organism->chromosome[m_chromosome].dbl_allele[allele] += ran_sample(&m_organism->chromosome[m_chromosome].rnd);
	}

	return;
}

void ebex_mutate_quaternion_singlepoint_drift(population_t *m_pop __attribute__((unused)), organism_t *m_organism, int m_chromosome)
{
	int allele;

	if (!m_organism)
		die("Null pointer to entity structure passed");

	/* Select mutation locus. */
	allele = (int) ran_uint_uniform(m_organism->chromosome[m_chromosome].length);

	/// Tweak a single allele
	ebex_quat_randomize_population(&m_organism->chromosome[m_chromosome].q_allele[allele],
			&m_organism->chromosome[m_chromosome].rnd, 1);


	return;
}

void ebex_mutate_quaternion_singlepoint_randomize(population_t *m_pop __attribute__((unused)), organism_t *m_organism, int m_chromosome)
{
	int allele;

	if (!m_organism)
		die("Null pointer to entity structure passed");

	/// Select mutation locus
	allele = (int) ran_uint_uniform(m_organism->chromosome[m_chromosome].length);

	/// Generate random point on the unit 4-sphere (uniform distribution)
	ran_unit_sphere_md(m_organism->chromosome[m_chromosome].q_allele[allele].element, 1.0, NULL, 4);

	return;

}

void ebex_mutate_quaternion_multipoint(population_t *m_pop __attribute__((unused)), organism_t *m_organism, int m_chromosome)
{
	size_t allele;
	if (!m_organism)
		die("Null pointer to entity structure passed");


	for (allele = 0; allele < m_organism->chromosome[m_chromosome].length; allele++)
	{
		if (ran_bool_prob(m_organism->chromosome[m_chromosome].allele_mutation_prob))
		{
			/// Tweak a single allele
			ebex_quat_randomize_population(&m_organism->chromosome[m_chromosome].q_allele[allele],
					&m_organism->chromosome[m_chromosome].rnd, 1);
		}
	}

	return;
}


void ebex_mutate_quaternion_allpoint(population_t *m_pop __attribute__((unused)), organism_t *m_organism, int m_chromosome)
{
	if (!m_organism)
		die("Null pointer to entity structure passed");


	/// Tweak each allele in the chromosome
	ebex_quat_randomize_population(m_organism->chromosome[m_chromosome].q_allele,
			&m_organism->chromosome[m_chromosome].rnd, m_organism->chromosome[m_chromosome].length);

	return;
}

static inline void ebex_genetic_mutate_drift(population_t *m_pop, organism_t *m_organism)
{
	for (size_t chromosome = 0; chromosome < m_pop->num_chromosomes; chromosome++)
	{
		if (ran_bool_prob(m_organism->chromosome[chromosome].allele_mutation_prob))
		{
			switch(m_organism->chromosome[chromosome].type)
			{
				case CHROMOSOME_TYPE_DOUBLE:
					ebex_mutate_double_singlepoint_drift(m_pop, m_organism, chromosome);
					break;
				case CHROMOSOME_TYPE_QUATERNION:
					ebex_mutate_quaternion_singlepoint_drift(m_pop, m_organism, chromosome);
					break;
				default:
					die("Invalid chromosome type!");
					break;
			}
		}
	}
}

static inline void ebex_genetic_mutate_randomize(population_t *m_pop, organism_t *m_organism)
{
	int chromosome = ran_uint_uniform(m_pop->num_chromosomes);

	switch(m_organism->chromosome[chromosome].type)
	{
		case CHROMOSOME_TYPE_DOUBLE:
			ebex_mutate_double_singlepoint_randomize(m_pop, m_organism, chromosome);
			break;
		case CHROMOSOME_TYPE_QUATERNION:
			ebex_mutate_quaternion_singlepoint_randomize(m_pop, m_organism, chromosome);
			break;
		default:
			die("Invalid chromosome type!");
			break;
	}
}

static inline void ebex_genetic_mutate_multipoint(population_t *m_pop, organism_t *m_organism)
{

	for (size_t chromosome = 0; chromosome < m_pop->num_chromosomes; chromosome++)
	{
		switch(m_organism->chromosome[chromosome].type)
		{
			case CHROMOSOME_TYPE_DOUBLE:
				ebex_mutate_double_multipoint(m_pop, m_organism, chromosome);
				break;
			case CHROMOSOME_TYPE_QUATERNION:
				ebex_mutate_quaternion_multipoint(m_pop, m_organism, chromosome);
				break;
			default:
				die("Invalid chromosome type!");
				break;
		}
	}
}

static inline void ebex_genetic_mutate_allpoint(population_t *m_pop, organism_t *m_organism)
{

	for (size_t chromosome = 0; chromosome < m_pop->num_chromosomes; chromosome++)
	{
		switch(m_organism->chromosome[chromosome].type)
		{
			case CHROMOSOME_TYPE_DOUBLE:
				ebex_mutate_double_allpoint(m_pop, m_organism, chromosome);
				break;
			case CHROMOSOME_TYPE_QUATERNION:
				ebex_mutate_quaternion_allpoint(m_pop, m_organism, chromosome);
				break;
			default:
				die("Invalid chromosome type!");
				break;
		}
	}
}

void ebex_genetic_mutate(population_t *m_pop, organism_t *m_organism, e_genetic_mutation_type m_type)
{

	switch(m_type)
	{
		case genetic_mutation_type_drift:
			ebex_genetic_mutate_drift(m_pop, m_organism);
			break;
		case genetic_mutation_type_randomize:
			ebex_genetic_mutate_randomize(m_pop, m_organism);
			break;
		case genetic_mutation_type_multipoint:
			ebex_genetic_mutate_multipoint(m_pop, m_organism);
			break;
		case genetic_mutation_type_allpoint:
			ebex_genetic_mutate_allpoint(m_pop, m_organism);
			break;
		default:
			die("Invalid Mutation type!");
			break;
	}
}
